#include "Behaviours.h"


Behaviours::Behaviours()
{
	MaxSpeed = 100.0;
	PathFindOn = false;
	SeekOn = false;
	FleeOn = false;
	ArrivalOn = false;
}

 void Behaviours::Initialise(XMFLOAT3 pos, XMFLOAT3 vel)
{
	Position = pos;
	Velocity = vel;
}

//checks behaviours and returns accleration
XMFLOAT3 Behaviours::AccumulateBehaviours()
{
	XMFLOAT3 acceleration = XMFLOAT3(0.0f,0.0f,0.0f);

	//checks bools for behaviors in order of importance and runs when neccassary
	if(FleeOn && (GetMagnitude(acceleration) <= MaxAcceleration))
	{
		XMFLOAT3 temp = Flee(TargetPosition);
		acceleration.x += temp.x;
		acceleration.y += temp.y;
		acceleration.z += temp.z;
	}
	if(SeekOn && (GetMagnitude(acceleration)  <= MaxAcceleration))
	{
		XMFLOAT3 temp = Seek(TargetPosition);
		acceleration.x += temp.x;
		acceleration.y += temp.y;
		acceleration.z += temp.z;
	}
	if(PathFindOn && (GetMagnitude(acceleration)  <= MaxAcceleration))
	{
		XMFLOAT3 temp = PathFind();
		acceleration.x += temp.x;
		acceleration.y += temp.y;
		acceleration.z += temp.z;
	}
	if(ArrivalOn && (GetMagnitude(acceleration)  <= MaxAcceleration))
	{
		
		XMFLOAT3 temp = Arrival(TargetPosition);
		acceleration.x += temp.x;
		acceleration.y += temp.y;
		acceleration.z += temp.z;
	}

	return acceleration;

}

//Acclerates to a certain point
XMFLOAT3 Behaviours::Seek(XMFLOAT3 Target)
{
	XMFLOAT3 DesiredVelocity;
	DesiredVelocity.x = Target.x - Position.x;
	DesiredVelocity.z = Target.z - Position.z;

	float divideBy = GetMagnitude(DesiredVelocity); 

	if(divideBy == 0 )
	{
		DesiredVelocity = XMFLOAT3(0.0f,0.0f,0.0f);
	}
	else
	{
		DesiredVelocity.x /= divideBy;
		DesiredVelocity.z /= divideBy;
	}

	DesiredVelocity.x *= MaxSpeed;
	DesiredVelocity.z *= MaxSpeed;

	XMFLOAT3 NewVelocity;
	NewVelocity.x = (DesiredVelocity.x - Velocity.x);
	NewVelocity.z = (DesiredVelocity.z - Velocity.z);

	return NewVelocity;
}

//Acclerates away from a certain point
XMFLOAT3 Behaviours::Flee(XMFLOAT3 Target)
{

	XMFLOAT3 DesiredVelocity;
	DesiredVelocity.x = Target.x - Position.x;
	DesiredVelocity.z = Target.z - Position.z;

	float divideBy = GetMagnitude(DesiredVelocity); 

	if(divideBy == 0 )
	{
		DesiredVelocity = XMFLOAT3(0.0f,0.0f,0.0f);
	}
	else
	{
		DesiredVelocity.x /= divideBy;
		DesiredVelocity.z /= divideBy;
	}

	DesiredVelocity.x *= MaxSpeed;
	DesiredVelocity.z *= MaxSpeed;

	XMFLOAT3 NewVelocity;
	NewVelocity.x = (DesiredVelocity.x - Velocity.x)*-1;
	NewVelocity.z = (DesiredVelocity.z - Velocity.z)*-1;

	return NewVelocity;
}

//Acclerates to a certain point then stops
XMFLOAT3 Behaviours::Arrival(XMFLOAT3 Target)
{
	XMFLOAT3 TargetOffset; 
	TargetOffset.x = Target.x - Position.x;
	TargetOffset.z = Target.z - Position.z;

	float Distance = GetMagnitude(TargetOffset);
	
	float RampedSpeed = MaxSpeed * (Distance / 100);

	float ClippedSpeed;

	if(Distance < 100)
	{
		ClippedSpeed = RampedSpeed;
	}
	else
	{
		ClippedSpeed = MaxSpeed;
	}

	XMFLOAT3 DesiredVelocity;
	DesiredVelocity.x = (ClippedSpeed / Distance) * TargetOffset.x;
	DesiredVelocity.z = (ClippedSpeed / Distance) * TargetOffset.z;

	XMFLOAT3 NewVelocity;
	NewVelocity.x = DesiredVelocity.x - Velocity.x;
	NewVelocity.z = DesiredVelocity.z - Velocity.z;

	return NewVelocity;
}

//follows path
XMFLOAT3 Behaviours::PathFind()
{

	XMFLOAT3 acceleration = XMFLOAT3(0.0f,0.0f,0.0f);

	//is path is not empty
	if(!Patrol.empty())
	{
		//seek target node
		acceleration = Seek(TargetPosition);

		//if bot is inside nodes hitbox
		if(GetDistance(TargetPosition,Position) < 20.0f)
		{			
			if((PatrolPoint+1) < Patrol.size())
			{
				PatrolPoint++;
				TargetPosition = Patrol[PatrolPoint];
			}
			else
			{
				TargetPosition = Patrol[0];
				PatrolPoint = 0;
			}
		}
	}

	return acceleration;
}

void Behaviours::SetTargetClosestPatrolPoint()
{
	//set intial point
	XMFLOAT3 Closest = Patrol[0];

	int closestpoint = 0;
	double LowestDistance = GetDistance(Closest,Position);

	//loop through patrol vector and compare distance between enemy and the points
	for(int i = 0; i < Patrol.size(); i++)
	{
		if(GetDistance(Patrol[i],Position) < LowestDistance)
		{
			closestpoint = i;
			Closest = Patrol[i];
			LowestDistance = GetDistance(Patrol[i],Position);
		}
	}
		
	//set the closest point as the current target 
	TargetPosition = Closest;

	//store index number of that point
	PatrolPoint = closestpoint;
}

float Behaviours::GetDistance(XMFLOAT3 A,XMFLOAT3 B)
{
	float distance, tempx,tempz;

	tempx = (A.x - B.x);
	tempx = tempx * tempx; 
	tempz = (A.z - B.z);
	tempz = tempz * tempz;

	distance = tempx + tempz;
	distance = sqrt(distance);

	return distance;
}

float Behaviours::GetMagnitude(XMFLOAT3 a)
{
	return (sqrt(a.x*a.x + a.z*a.z));
}

///////////////////GETS AND SETS //////////////////////
void Behaviours::SetPath(std::vector<XMFLOAT3> New)
{
	Patrol = New;
	PatrolPoint = 0;
}

void Behaviours::SetTargetPosition(XMFLOAT3 New)
{
	TargetPosition = New;
}

void Behaviours::SetPathFindOn(bool New)
{
	PathFindOn = New;
}
void Behaviours::SetSeekOn(bool New)
{
	SeekOn = New;
}
void Behaviours::SetFleeOn(bool New)
{
	FleeOn = New;
}
void Behaviours::SetArrivalOn(bool New)
{
	ArrivalOn = New;
}

bool Behaviours::GetPathFindOn()
{
	return PathFindOn;
}
bool Behaviours::GetSeekOn()
{
	return SeekOn;
}
bool Behaviours::GetFleeOn()
{
	return FleeOn;
}
bool Behaviours::GetArrivalOn()
{
	return ArrivalOn;
}

void Behaviours::SetMaxSpeeds(float speed,float accel)
{
	MaxSpeed = speed;
	MaxAcceleration = accel;
}